package nezumi.planning;

import java.awt.Point;
import java.util.Comparator;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.PriorityQueue;
import java.util.Queue;

import nezumi.state.MapState;

/**
 * An A* implementation for path finding using a simple distance-based
 * heuristic.
 * 
 * @author rui
 * 
 */
public class AStar {

	protected static final CostComparator COST_CMP = new CostComparator();

	protected final int VERTICAL_COST = 10;

	protected final int HORIZONTAL_COST = 10;

	protected final int DIAGONAL_COST = (int) Math.rint(Math.sqrt(VERTICAL_COST
			* VERTICAL_COST + HORIZONTAL_COST * HORIZONTAL_COST));

	protected final MapState mapState;
	protected final Node origin;
	protected final Node destination;

	protected final Queue<Node> open;
	protected final int[] closed;

	public AStar(MapState mapState, Point origin, Point destination) {
		assert mapState != null : "mapState = " + mapState;

		this.mapState = mapState;
		this.origin = new Node(origin);
		this.destination = new Node(destination);

		int width = mapState.getWidth();
		int height = mapState.getHeight();

		open = new PriorityQueue<Node>(Math.max(width, height) * 2, COST_CMP);
		closed = new int[(width * height >> 5) + 1];
	}

	/**
	 * Adds the node at {@code x}, {@code y} to the open list, using
	 * {@code parent} as the parent.
	 * 
	 * <p>
	 * If the node was already added to the open list, the old value is either
	 * kept or replaced, depending on whether the old one is closer to the
	 * origin or not.
	 * </p>
	 * 
	 * @param x
	 * @param y
	 * @param parent
	 */
	protected void addToOpen(int x, int y, Node parent) {
		Node openNode = new Node(x, y);
		openNode.setParent(parent);

		replacing: for (Iterator<Node> i = open.iterator(); i.hasNext();) {
			Node existing = i.next();
			if (existing.x == x && existing.y == y) {
				if (existing.gcost > openNode.gcost) {
					i.remove();
					break replacing;
				} else {
					return;
				}
			}
		}

		openNode.computeHeuristic();
		open.add(openNode);
	}

	/**
	 * Starts the algorithm and returns true if a valid path was found.
	 * 
	 * @return
	 */
	public boolean findPath() {
		Node current = origin;
		while (current != null
				&& (current.x != destination.x || current.y != destination.y)) {
			process(current);
			current = open.poll();
		}

		if (current != null) {
			if (current.parent != null)
				destination.setParent(current.parent);
			else
				current = null;
		}

		return current != null;
	}

	public LinkedList<Point> getPath() {
		assert destination.parent != null
				|| (destination.x == origin.x && destination.y == origin.y);

		LinkedList<Point> path = new LinkedList<Point>();
		Node current = destination;

		while (current != null) {
			path.addFirst(new Point(current.x, current.y));
			current = current.parent;
		}

		return path;
	}

	/**
	 * Checks whether a node was already processed.
	 * 
	 * @param x
	 * @param y
	 * @return
	 */
	protected boolean isClosed(int x, int y) {
		int i = mapState.getWidth() * y + x;
		return (closed[i >> 5] & (1 << (i & 31))) != 0;
	}

	/**
	 * Processes the passed node.
	 * 
	 * <ul>
	 * <li>Adds it to the closed list.</li>
	 * <li>Adds all adjacent nodes to the open list, if it was not processed
	 * yet and is walkable.</li>
	 * </ul>
	 * 
	 * @param node
	 */
	protected void process(Node node) {
		// no need to process it twice
		setClosed(node.x, node.y);

		int width = mapState.getWidth();
		int height = mapState.getHeight();

		// respect the array bounds
		int lX = node.x == 0 ? 0 : node.x - 1;
		int uX = node.x >= width - 1 ? width - 1 : node.x + 1;
		int lY = node.y == 0 ? 0 : node.y - 1;
		int uY = node.y >= height - 1 ? height - 1 : node.y + 1;

		// check all the neighbors
		for (int x = lX; x <= uX; ++x) {
			for (int y = lY; y <= uY; ++y) {
				if (!isClosed(x, y) && mapState.isWalkable(x, y)) {
					addToOpen(x, y, node);
				}
			}
		}
	}

	/**
	 * Sets the node at {@code x}, {@code y} to "already been processed".
	 * 
	 * @param x
	 * @param y
	 */
	protected void setClosed(int x, int y) {
		int i = mapState.getWidth() * y + x;
		closed[i >> 5] |= (1 << (i & 31));
	}

	protected class Node {
		final int x;
		final int y;

		Node parent;
		int gcost;
		int hcost;

		public Node(int x, int y) {
			assert x >= 0 && x < mapState.getWidth() : "x = " + x;
			assert y >= 0 && y < mapState.getHeight() : "y = " + x;

			this.x = x;
			this.y = y;
		}

		public Node(Point point) {

			assert point != null : "point = " + point;

			assert point.x >= 0 && point.x < mapState.getWidth() : "x = "
					+ point.x;
			assert point.y >= 0 && point.y < mapState.getHeight() : "y = "
					+ point.y;

			this.x = point.x;
			this.y = point.y;
		}

		public void computeHeuristic() {
			hcost = (Math.abs(x - destination.x) + Math.abs(y - destination.y)) // 
					* (VERTICAL_COST + HORIZONTAL_COST) / 2;
		}

		public void setParent(Node parent) {
			this.parent = parent;
			if (parent.x == x) {
				gcost = parent.gcost + HORIZONTAL_COST;
			} else if (parent.y == y) {
				gcost = parent.gcost + VERTICAL_COST;
			} else {
				gcost = parent.gcost + DIAGONAL_COST;
			}
		}

		@Override
		public String toString() {
			return String.format("(%d,%d):%s)", x, y, super.toString());
		}
	}

	private static class CostComparator implements Comparator<Node> {

		public int compare(Node nodeA, Node nodeB) {
			return (nodeA.gcost + nodeA.hcost) - (nodeB.gcost + nodeB.hcost);
		}
	}
}